#include "Matrix4x4.h"
#include "MathBase.h"

CString SMatrix4x4::ToString() const
{
    return U"Matrix4x4:[\n"
    +CString(Data[0][0])+U","+CString(Data[1][0])+U","+CString(Data[2][0])+U","+CString(Data[3][0])+U"\n"
    +CString(Data[0][1])+U","+CString(Data[1][1])+U","+CString(Data[2][1])+U","+CString(Data[3][1])+U"\n"
    +CString(Data[0][2])+U","+CString(Data[1][2])+U","+CString(Data[2][2])+U","+CString(Data[3][2])+U"\n"
    +CString(Data[0][3])+U","+CString(Data[1][3])+U","+CString(Data[2][3])+U","+CString(Data[3][3])+U"\n"
    +U"]";    
}

SMatrix4x4 SMatrix4x4::LookAt(const SVec3 & InCameraPosition
			,const SVec3 & InForwardDirection
			,const SVec3& InCameraUpDirection )
{
    SVec3 Forward= InForwardDirection;
    Forward.Normalize();

    SVec3 Up=InCameraUpDirection;
    Up.Normalize();

    SVec3 Right=Forward.CrossProduct(Up);
    Up=Right.CrossProduct(Forward);

    SMatrix4x4 Ret;

    Ret.Data[0][0]=Right.X;
    Ret.Data[1][0]=Right.Y;
    Ret.Data[2][0]=Right.Z;

    Ret.Data[0][1]=Forward.X;
    Ret.Data[1][1]=Forward.Y;
    Ret.Data[2][1]=Forward.Z;
    
    Ret.Data[0][2]=Up.X;
    Ret.Data[1][2]=Up.Y;
    Ret.Data[2][2]=Up.Z;

    Ret.Data[3][0]=-Right.DotProduct(InCameraPosition);
    Ret.Data[3][1]=-Forward.DotProduct(InCameraPosition);
    Ret.Data[3][2]=-Up.DotProduct(InCameraPosition);

    Ret.Data[0][3]=0;
    Ret.Data[1][3]=0;
    Ret.Data[2][3]=0;
    Ret.Data[3][3]=1;    

    return Ret;   
}


// glm::mat4x4 SMatrix4x4::ToGlm() const
// {
//     glm::mat4x4 Ret;
//     for (int Row = 0; Row < 4; Row++)
//     {
//         for (int Column = 0; Column < 4; Column++)
//         {
//             Ret[Column][Row] = Data[Column][Row];
//         }
//     }
//     return Ret;

// }

SMatrix4x4 SMatrix4x4::Perspective(Float InFovY,Float InAspect,Float InNear,Float InFar)
{
    //camera coordinate:
    // Forward Y+
    // Right   X+
    // Up      Z+

    //NDC : (defined by vulkan)
    //Forward Z+
    //Right   X+
    //Up      Y-
    
    //perspective matrix need convert Camera coordinate to NDC
    SMatrix4x4 Perspective;

    Float HalfTanFOVY= CMathBase::Tan(InFovY/2);

    Perspective.Data[0][0]=1/(InAspect*HalfTanFOVY);
    Perspective.Data[1][0]=0;
    Perspective.Data[2][0]=0;
    Perspective.Data[3][0]=0; 

    Perspective.Data[0][1]=0;
    Perspective.Data[1][1]=0;
    Perspective.Data[2][1]=-1/HalfTanFOVY;
    Perspective.Data[3][1]=0; 

    Float A=InFar/(InFar-InNear);
    Float B=-A*InNear;
    Perspective.Data[0][2]=0;
    Perspective.Data[1][2]=A;
    Perspective.Data[2][2]=0;
    Perspective.Data[3][2]=B;

    Perspective.Data[0][3]=0;
    Perspective.Data[1][3]=1;
    Perspective.Data[2][3]=0;
    Perspective.Data[3][3]=0;    

    return   Perspective;  

}

SMatrix4x4 SMatrix4x4::Orthographic(Float left, Float right, Float bottom, Float top, Float zNear, Float zFar)
{
    //camera coordinate:
    // Forward Y+
    // Right   X+
    // Up      Z+

    //NDC : (defined by vulkan)
    //Forward Z+
    //Right   X+
    //Up      Y-
    
    //Ortho matrix need convert Camera point (x,y,z) to NDC point (x',y',z')

    //(x-left) /(right -left) = (x' -(-1)) / (1-(-1))
    // (z-bottom)/ (top-bottom)= (1-y') / (1-(-1))
    // (y-near) /(far-near) =z' / (1-0)  
    
    //so,
    // x'= 2/(right-left)  * x   + (-right -left) /(right-left)
    // y'= -2/(top-bottom) * z    + (top+bottom) / (top-bottom)
    // z'= y/(far-near) - near/(far-near)

    //write above in matrix format:

    // x'      2/(r-l)    0       0         (-r-l)/(r-l)        x
    // y'      0          0       2/(t-b)   (t-3b)/(t-b)      * y
    // z'  =   0          1/(f-n) 0         -n/(f-n)            z
    // 1       0          0       0         1                   1


    SMatrix4x4 Result;
		Result.Data[0][0] = 2 / (right - left);
		Result.Data[2][1] = -2 / (top - bottom);
		Result.Data[1][2] =  1/ (zFar - zNear);

		Result.Data[3][0] =  (-right - left) / (right - left);
		Result.Data[3][1] =  (top + bottom) / (top - bottom);
		Result.Data[3][2] = - zNear / (zFar - zNear);
        Result.Data[3][3] = 1 ;

    return Result;
}

void SMatrix4x4::ToFloatArray(float OutArray[16]) const
{
    // for (int Row = 0; Row < 4; Row++)
    // {
    //     for (int Column = 0; Column < 4; Column++)
    //     {
    //         OutArray[Row+Column*4]=Data[Column][Row];
    //     }
    // }

    OutArray[0]=Data[0][0];
    OutArray[1]=Data[0][1];
    OutArray[2]=Data[0][2];
    OutArray[3]=Data[0][3];

    OutArray[4]=Data[1][0];
    OutArray[5]=Data[1][1];
    OutArray[6]=Data[1][2];
    OutArray[7]=Data[1][3];

    OutArray[8]=Data[2][0];
    OutArray[9]=Data[2][1];
    OutArray[10]=Data[2][2];
    OutArray[11]=Data[2][3];

    OutArray[12]=Data[3][0];
    OutArray[13]=Data[3][1];
    OutArray[14]=Data[3][2];
    OutArray[15]=Data[3][3];
}

SVec4 SMatrix4x4::operator*(const SVec4& InValue) const
{
    SVec4 Ret;

    Ret.X=Data[0][0]*InValue.X+Data[1][0]*InValue.Y+Data[2][0]*InValue.Z+Data[3][0]*InValue.W;

    Ret.Y=Data[0][1]*InValue.X+Data[1][1]*InValue.Y+Data[2][1]*InValue.Z+Data[3][1]*InValue.W;
    
    Ret.Z=Data[0][2]*InValue.X+Data[1][2]*InValue.Y+Data[2][2]*InValue.Z+Data[3][2]*InValue.W;
    
    Ret.W=Data[0][3]*InValue.X+Data[1][3]*InValue.Y+Data[2][3]*InValue.Z+Data[3][3]*InValue.W;

    return Ret;
}

SVec3 SMatrix4x4::operator*(const SVec3& InValue) const
{
    SVec4 Result=this->operator*(SVec4(InValue,1.f));
    return SVec3(Result.X,Result.Y,Result.Z);
}

bool SMatrix4x4::operator==(const SMatrix4x4 &InOther) const
{
    for (int Row = 0; Row < 4; Row++)
    {
        for (int Column = 0; Column < 4; Column++)
        {
            if (Data[Column][Row]!=InOther.Data[Column][Row])
            {
                return false;
            }
        }
    }
    return true;
}

SMatrix4x4 SMatrix4x4::Inverse() const
{
    SMatrix4x4 Ret;

    Float A=Data[0][0]*Data[1][1]-Data[0][1]*Data[1][0];
    Float B=Data[0][0]*Data[1][2]-Data[0][2]*Data[1][0];
    Float C=Data[0][0]*Data[1][3]-Data[0][3]*Data[1][0];
    Float D=Data[0][1]*Data[1][2]-Data[0][2]*Data[1][1];
    Float E=Data[0][1]*Data[1][3]-Data[0][3]*Data[1][1];
    Float F=Data[0][2]*Data[1][3]-Data[0][3]*Data[1][2];
    Float G=Data[2][0]*Data[3][1]-Data[2][1]*Data[3][0];
    Float H=Data[2][0]*Data[3][2]-Data[2][2]*Data[3][0];
    Float I=Data[2][0]*Data[3][3]-Data[2][3]*Data[3][0];
    Float J=Data[2][1]*Data[3][2]-Data[2][2]*Data[3][1];
    Float K=Data[2][1]*Data[3][3]-Data[2][3]*Data[3][1];
    Float L=Data[2][2]*Data[3][3]-Data[2][3]*Data[3][2];

    Float Det=A*L-B*K+C*J+D*I-E*H+F*G;

    if(Det==0)
    {
        return SMatrix4x4::Identity();
    }

    Float InvDet=1.f/Det;

    Ret.Data[0][0]=(Data[1][1]*L-Data[1][2]*K+Data[1][3]*J)*InvDet;
    Ret.Data[0][1]=(-Data[0][1]*L+Data[0][2]*K-Data[0][3]*J)*InvDet;
    Ret.Data[0][2]=(Data[3][1]*F-Data[3][2]*E+Data[3][3]*D)*InvDet;
    Ret.Data[0][3]=(-Data[2][1]*F+Data[2][2]*E-Data[2][3]*D)*InvDet;

    Ret.Data[1][0]=(-Data[1][0]*L+Data[1][2]*I-Data[1][3]*H)*InvDet;
    Ret.Data[1][1]=(Data[0][0]*L-Data[0][2]*I+Data[0][3]*H)*InvDet;
    Ret.Data[1][2]=(-Data[3][0]*F+Data[3][2]*C-Data[3][3]*B)*InvDet;
    Ret.Data[1][3]=(Data[2][0]*F-Data[2][2]*C+Data[2][3]*B)*InvDet;

    Ret.Data[2][0]=(Data[1][0]*K-Data[1][1]*I+Data[1][3]*G)*InvDet;
    Ret.Data[2][1]=(-Data[0][0]*K+Data[0][1]*I-Data[0][3]*G)*InvDet;
    Ret.Data[2][2]=(Data[3][0]*E-Data[3][1]*C+Data[3][3]*A)*InvDet;
    Ret.Data[2][3]=(-Data[2][0]*E+Data[2][1]*C-Data[2][3]*A)*InvDet;

    Ret.Data[3][0]=(-Data[1][0]*J+Data[1][1]*H-Data[1][2]*G)*InvDet;
    Ret.Data[3][1]=(Data[0][0]*J-Data[0][1]*H+Data[0][2]*G)*InvDet;
    Ret.Data[3][2]=(-Data[3][0]*D+Data[3][1]*B-Data[3][2]*A)*InvDet;
    Ret.Data[3][3]=(Data[2][0]*D-Data[2][1]*B+Data[2][2]*A)*InvDet;

    return Ret;
}

SMatrix4x4 SMatrix4x4::Transpose() const
{
    SMatrix4x4 Ret;

    Ret.Data[0][0]=Data[0][0];
    Ret.Data[0][1]=Data[1][0];
    Ret.Data[0][2]=Data[2][0];
    Ret.Data[0][3]=Data[3][0];

    Ret.Data[1][0]=Data[0][1];
    Ret.Data[1][1]=Data[1][1];
    Ret.Data[1][2]=Data[2][1];
    Ret.Data[1][3]=Data[3][1];

    Ret.Data[2][0]=Data[0][2];
    Ret.Data[2][1]=Data[1][2];
    Ret.Data[2][2]=Data[2][2];
    Ret.Data[2][3]=Data[3][2];

    Ret.Data[3][0]=Data[0][3];
    Ret.Data[3][1]=Data[1][3];
    Ret.Data[3][2]=Data[2][3];
    Ret.Data[3][3]=Data[3][3];

    return Ret;
}

SMatrix4x4 CreateIdentity()
{
    SMatrix4x4 Ret;

    Ret.Data[0][0]=1.f;
    Ret.Data[1][1]=1.f;
    Ret.Data[2][2]=1.f;
    Ret.Data[3][3]=1.f;   

    return Ret;
}


SMatrix4x4 SMatrix4x4::Identity()
{
    static SMatrix4x4 UnitMatrix=CreateIdentity();

    return UnitMatrix;

}